﻿#pragma once

#include <iostream>
#include <fstream>
#include <opencv2/opencv.hpp>
#include <Eigen/Dense> 
#include <Eigen/Core>
#include <Eigen/Geometry>// Eigen 几何模块
#include<vector>


class AlgorithmVision
{
public:
    AlgorithmVision() = default;

public:

#define RMToRV Rodrigues
//旋转矢量转旋转矩阵
#define RVToRM Rodrigues

//由旋转矢量和平移矢量创建3D齐次阵
	void HomMat3dFromRVT(const cv::Mat& Rvec, const cv::Mat& Tvec, cv::Mat* HomMat3d);
	//分解3D齐次阵为旋转矢量和平移矢量
	void HomMat3dToRVT(const cv::Mat& HomMat3d, cv::Mat* Rvec, cv::Mat* Tvec);

	//由旋转阵和平移矢量创建3D齐次阵
	void HomMat3dFromRT(const cv::Mat& RMatrix, const cv::Mat& Tvec, cv::Mat* HomMat3d);

	//分解3D齐次阵为旋转阵和平移矢量
	void HomMat3dToRT(const cv::Mat& HomMat3d, cv::Mat* RMatrix, cv::Mat* Tvec);

	//齐次矩阵相乘
	void HomMat3dCompose(const cv::Mat& HomMat3DLeft, const cv::Mat& HomMat3DRight, cv::Mat* HomMat3DCompose);

	//绘制函数
	//绘制空间坐标系
	void Disp3dCoordSystem(cv::Mat* DrawImage, const cv::Mat& Rvec, const cv::Mat& Tvec, const cv::Mat& CameraMatrix, const cv::Mat& DistCoeffs, const float& fAxialLength = 100, const int& iAxiaThickness = 3, const std::string& sCoordName = "", const bool& bShowCoordPoseText = false);

	void Disp3dCoordSystem(cv::Mat* DrawImage, const cv::Mat& HomMat3dPose, const cv::Mat& CameraMatrix, const cv::Mat& DistCoeffs, const float& fAxialLength = 100, const int& iAxiaThickness = 3, const std::string& sCoordName = "", const bool& bShowCoordPoseText = false);

	//绘制一些线
	void lines(cv::InputOutputArray img, const std::vector<cv::Point>& vpt1, const std::vector<cv::Point>& vpt2, const cv::Scalar& color = cv::Scalar(255, 0, 0), int thickness = 1, int lineType = cv::LINE_8, int shift = 0);
	void lines(cv::InputOutputArray img, const std::vector<cv::Point2f>& vpt1, const std::vector<cv::Point2f>& vpt2, const cv::Scalar& color = cv::Scalar(255, 0, 0), int thickness = 1, int lineType = cv::LINE_8, int shift = 0);


};
